/*
 * drive_server.cpp
 *
 *  Created on: Jan 17, 2013
 *      Author: michiel
 */

#include "servers/drive_server.h"

DriveAction::DriveAction(std::string name) : BaseServer(name),
	_as(nh_, name, boost::bind(&DriveAction::execute, this, _1), false)
{
	controller_sub_ = nh_.subscribe(Topic::PS3_JOY, 1, &DriveAction::controllerCallback, this);
	_as.start();
}

DriveAction::~DriveAction() {
}

void DriveAction::controllerCallback(const sensor_msgs::Joy::ConstPtr &msg) {
	double rightTrigger = msg->axes[PS3_AXIS_BUTTON_REAR_RIGHT_2] * msg->axes[PS3_AXIS_BUTTON_REAR_RIGHT_2];
	double leftTrigger	= msg->axes[PS3_AXIS_BUTTON_REAR_LEFT_2] * msg->axes[PS3_AXIS_BUTTON_REAR_LEFT_2];
	forward_vel_ = (rightTrigger - leftTrigger) * 0.5;
	turn_rate_ = msg->axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS];
	turn_rate_ *= turn_rate_ * turn_rate_;
}

void DriveAction::execute(const rp_states::DriveGoalConstPtr &goal) {
	_result.stopped = false;
	if (goal->has_ball) {
		// TODO Move trunk up or down based on ball status.
	}

	while (ros::ok() && !(_as.isPreemptRequested()))
	{
		geometry_msgs::Twist twist;
		twist.linear.x = forward_vel_;
		twist.angular.z = turn_rate_ * 0.5;
		publishWheelsSpeed(twist);
	}

	// set wheel speed to 0
	geometry_msgs::Twist twist;
	twist.linear.x	= 0.0;
	twist.angular.z	= 0;
	publishWheelsSpeed(twist);


	_result.stopped = true;
	_as.setSucceeded(_result);
}
